#include <iostream>
#include <string>
#include <iomanip>
#include <sstream>
#include <filesystem>
#include <opencv2/opencv.hpp>
#include <cstdlib>   // 用于system命令
#include <thread>    // 用于睡眠
#include <chrono>    // 用于时间间隔
#include <algorithm> // 用于find_if
#include <cstdio>    // 用于popen
#include <memory>    // 用于unique_ptr
#include <stdexcept> // 用于runtime_error
#include <fcntl.h>   // 用于fcntl, O_NONBLOCK
#include <termios.h> // 用于termios, tcgetattr, tcsetattr
#include <unistd.h>  // 用于STDIN_FILENO, read

// 命名空间简化
namespace fs = std::filesystem;
using namespace cv;
using namespace std;

// 摄像头配置结构体
struct CameraConfig
{
    int index;
    string name;
    bool detected;
    string tempFile;
};

// 分辨率配置
struct ResolutionConfig
{
    int width;
    int height;
    string name;
};

// 创建目录函数
bool createDirectory(const string &path)
{
    try
    {
        if (!fs::exists(path))
        {
            return fs::create_directory(path);
        }
        return true;
    }
    catch (const fs::filesystem_error &e)
    {
        cerr << "Error creating directory: " << e.what() << endl;
        return false;
    }
}

// 执行命令并获取输出
string executeCommand(const string &cmd)
{
    array<char, 128> buffer;
    string result;
    unique_ptr<FILE, decltype(&pclose)> pipe(popen(cmd.c_str(), "r"), pclose);
    if (!pipe)
    {
        throw runtime_error("popen() failed!");
    }
    while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr)
    {
        result += buffer.data();
    }
    return result;
}

// 使用libcamera-still捕获图像并转换为OpenCV Mat
bool captureWithLibcamera(CameraConfig &camera, const string &filename, Mat &outputImage,
                          int timeoutMs = 1000, int width = 0, int height = 0)
{
    // 构建命令 - 添加输出重定向到/dev/null以防止libcamera日志刷屏
    string command = "rpicam-still -t " + to_string(timeoutMs) + " --camera ";
    command += to_string(camera.index);
    command += " -o " + filename + " --nopreview --immediate";

    // 如果指定了分辨率，添加分辨率参数
    if (width > 0 && height > 0)
    {
        command += " --width " + to_string(width) + " --height " + to_string(height);
    }

    command += " 2>/dev/null";

    // 执行命令
    int ret = system(command.c_str());
    if (ret != 0)
    {
        cerr << "Error capturing image for " << camera.name
             << " camera (index " << camera.index << "). Return code: " << ret << endl;
        return false;
    }

    // 检查文件是否成功创建
    if (!fs::exists(filename))
    {
        cerr << "Captured image file does not exist: " << filename << endl;
        return false;
    }

    // 使用OpenCV读取保存的图像
    outputImage = imread(filename);
    if (outputImage.empty())
    {
        cerr << "Error reading captured image: " << filename << endl;
        return false;
    }

    return true;
}

// 获取已存在图像的最大编号
int getMaxImageIndex(const string &directory, const string &prefix)
{
    int max_index = -1;
    try
    {
        for (const auto &entry : fs::directory_iterator(directory))
        {
            string filename = entry.path().filename().string();
            if (filename.find(prefix) == 0 && filename.find(".jpg") != string::npos)
            {
                try
                {
                    // 提取"left_0000.jpg"中的数字部分
                    size_t startPos = filename.find('_') + 1;
                    size_t endPos = filename.find('.');
                    string numStr = filename.substr(startPos, endPos - startPos);
                    int num = stoi(numStr);
                    max_index = max(max_index, num);
                }
                catch (...)
                {
                    // 忽略非数字文件名
                }
            }
        }
    }
    catch (const fs::filesystem_error &e)
    {
        cerr << "Error accessing directory: " << e.what() << endl;
    }
    return max_index;
}

// 检测摄像头
bool detectCamera(CameraConfig &camera)
{
    string testFile = "test_cam_" + to_string(camera.index) + ".jpg";
    Mat testImage;

    // 尝试捕获图像
    bool detected = captureWithLibcamera(camera, testFile, testImage, 2000);

    // 清理测试文件
    remove(testFile.c_str());

    if (detected)
    {
        cout << camera.name << " camera (index " << camera.index << ") detected successfully" << endl;
    }
    else
    {
        cout << camera.name << " camera (index " << camera.index << ") not detected" << endl;
    }

    return detected;
}

// 非阻塞键盘输入检测
int getKeyNonBlocking()
{
    struct termios oldt, newt;
    int ch;
    int oldf;

    // 获取当前终端设置
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;

    // 设置终端为非规范模式
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);

    // 设置非阻塞读取
    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

    // 尝试读取按键
    ch = getchar();

    // 恢复终端设置
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    fcntl(STDIN_FILENO, F_SETFL, oldf);

    return ch;
}

// 显示分辨率选择菜单
void showResolutionMenu(const vector<ResolutionConfig> &resolutions, int currentIndex)
{
    cout << "\n=== Resolution Settings ===" << endl;
    for (size_t i = 0; i < resolutions.size(); i++)
    {
        if (i == currentIndex)
        {
            cout << "> [" << i + 1 << "] " << resolutions[i].name
                 << " (" << resolutions[i].width << "x" << resolutions[i].height << ")" << endl;
        }
        else
        {
            cout << "  [" << i + 1 << "] " << resolutions[i].name
                 << " (" << resolutions[i].width << "x" << resolutions[i].height << ")" << endl;
        }
    }
    cout << "===========================" << endl;
}

// 调整窗口大小以适应屏幕显示
void resizeWindowToFit(const string &winname, const Mat &image, int maxWidth = 1200, int maxHeight = 800)
{
    int width = image.cols;
    int height = image.rows;
    
    // 如果图像尺寸超过最大限制，则按比例缩放
    if (width > maxWidth || height > maxHeight)
    {
        double scale = min((double)maxWidth / width, (double)maxHeight / height);
        width = static_cast<int>(width * scale);
        height = static_cast<int>(height * scale);
    }
    
    resizeWindow(winname, width, height);
}

// 根据图像尺寸计算合适的文字大小、厚度和行间距
void getTextProperties(const Mat &image, double &fontScale, int &thickness, int &lineSpacing)
{
    int minDimension = min(image.rows, image.cols);
    
    // 根据图像的最小尺寸动态调整文字大小
    if (minDimension >= 2500) {
        fontScale = 3.0;
        thickness = 5;
        lineSpacing = 100;
    } else if (minDimension >= 2000) {
        fontScale = 2.5;
        thickness = 4;
        lineSpacing = 80;
    } else if (minDimension >= 1500) {
        fontScale = 2.0;
        thickness = 3;
        lineSpacing = 70;
    } else if (minDimension >= 1000) {
        fontScale = 1.5;
        thickness = 3;
        lineSpacing = 60;
    } else if (minDimension >= 600) {
        fontScale = 1.0;
        thickness = 2;
        lineSpacing = 50;
    } else {
        fontScale = 0.7;
        thickness = 2;
        lineSpacing = 40;
    }
}

// 添加带背景的文字到图像上，提高可读性
void putTextWithBackground(Mat &image, const string &text, Point position, 
                          double fontScale, int thickness, Scalar textColor, Scalar bgColor)
{
    int baseline = 0;
    Size textSize = getTextSize(text, FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
    
    // 绘制背景矩形，增加一些边距
    int margin = 10;
    rectangle(image, 
              Point(position.x - margin, position.y - textSize.height - margin),
              Point(position.x + textSize.width + margin, position.y + baseline + margin),
              bgColor, FILLED);
    
    // 绘制文字
    putText(image, text, position, FONT_HERSHEY_SIMPLEX, fontScale, textColor, thickness);
}

// 在图像上添加多行文字信息，自动调整位置避免重叠
void addImageInfo(Mat &image, const string &resolutionName, int width, int height, int counter)
{
    // 动态计算文字大小、厚度和行间距
    double fontScale;
    int thickness, lineSpacing;
    getTextProperties(image, fontScale, thickness, lineSpacing);
    
    // 准备显示的文字 - 使用英文避免乱码
    vector<string> texts = {
        "Res: " + resolutionName,
        "Size: " + to_string(width) + "x" + to_string(height),
        "Idx: " + to_string(counter)
    };
    
    // 计算文字起始位置 - 使用图像高度的百分比，确保在不同分辨率下都能正确显示
    int startY = image.rows * 0.05; // 从图像顶部5%的位置开始
    int startX = image.cols * 0.02; // 从图像左侧2%的位置开始
    
    // 文字颜色和背景颜色
    Scalar textColor(0, 255, 0); // 绿色文字
    Scalar bgColor(0, 0, 0);     // 黑色背景
    
    // 绘制每一行文字
    for (size_t i = 0; i < texts.size(); i++)
    {
        int yPos = startY + (i * lineSpacing);
        
        // 确保文字不会超出图像底部
        if (yPos < image.rows - 50) {
            putTextWithBackground(image, texts[i], Point(startX, yPos), 
                                fontScale, thickness, textColor, bgColor);
        }
    }
    
    // 在右下角添加状态指示器
    string statusText = "Preview";
    int statusY = image.rows - 30;
    int statusX = image.cols - 150;
    
    if (statusX > 0 && statusY > 0) {
        putTextWithBackground(image, statusText, Point(statusX, statusY), 
                            fontScale, thickness, textColor, bgColor);
    }
}

int main()
{
    // 设置libcamera日志级别只显示错误，减少日志输出
    setenv("LIBCAMERA_LOG_LEVELS", "*:ERROR", 1);

    cout << "Raspberry Pi 5 IMX219 Stereo Camera Calibration Image Capture" << endl;
    cout << "=============================================================" << endl;

    // 定义支持的分辨率（IMX219传感器支持的分辨率）
    vector<ResolutionConfig> resolutions = {
        {3280, 2464, "8MP"},
        {1920, 1080, "1080p Full HD"},
        {1640, 1232, "4MP"},
        {1280, 720, "720p HD"},
        {640, 480, "VGA"}};

    int currentResolutionIndex = 0; // 默认使用全分辨率

    // 显示分辨率选择菜单
    showResolutionMenu(resolutions, currentResolutionIndex);
    cout << "Current selection: " << resolutions[currentResolutionIndex].name << endl;

    // 创建保存图像的目录
    if (!createDirectory("left_images") || !createDirectory("right_images"))
    {
        cerr << "Error: Cannot create image save directories!" << endl;
        return -1;
    }

    // 初始化摄像头配置
    vector<CameraConfig> cameras = {
        {0, "Left Camera", false, "temp_left.jpg"},
        {1, "Right Camera", false, "temp_right.jpg"}};

    // 检测摄像头
    cout << "Detecting camera configuration..." << endl;
    for (auto &camera : cameras)
    {
        camera.detected = detectCamera(camera);
    }

    // 如果两个摄像头都未检测到，尝试交换索引
    if (!cameras[0].detected && !cameras[1].detected)
    {
        cout << "Trying to swap camera indices..." << endl;
        swap(cameras[0].index, cameras[1].index);

        for (auto &camera : cameras)
        {
            camera.detected = detectCamera(camera);
        }
    }

    if (!cameras[0].detected && !cameras[1].detected)
    {
        cerr << "Error: No cameras detected!" << endl;
        cerr << "Please check:" << endl;
        cerr << "1. Camera physical connection" << endl;
        cerr << "2. Enable camera interface in raspi-config" << endl;
        cerr << "3. Restart system and try again" << endl;
        return -1;
    }

    // 查找已存在的最高编号
    int max_left_index = getMaxImageIndex("left_images", "left_");
    int max_right_index = getMaxImageIndex("right_images", "right_");
    int image_counter = max(max_left_index, max_right_index) + 1;
    
    // 记录实际保存的图像数量
    int saved_pairs_count = 0;
    int saved_left_count = 0;
    int saved_right_count = 0;

    cout << "Starting image capture from index: " << image_counter << endl;
    cout << "Key controls:" << endl;
    cout << "  'c' - Save stereo image pair" << endl;
    cout << "  's' - Save single image (left)" << endl;
    cout << "  'r' - Save single image (right)" << endl;
    cout << "  'd' - Toggle debug mode" << endl;
    cout << "  'p' - Toggle preview mode" << endl;
    cout << "  '1'-'5' - Change resolution" << endl;
    cout << "  ESC - Exit program" << endl;
    cout << "=============================================================" << endl;

    // 创建显示窗口
    for (const auto &camera : cameras)
    {
        if (camera.detected)
        {
            namedWindow(camera.name, WINDOW_NORMAL);
            // 初始窗口大小设为中等大小，后续会根据实际图像调整
            resizeWindow(camera.name, 800, 600);
        }
    }

    vector<Mat> displayImages(cameras.size());
    bool debugMode = false;
    bool previewEnabled = true;
    bool firstPreview = true; // 标记是否是第一次预览，用于调整窗口大小

    cout << "Starting preview, press ESC to exit..." << endl;
    cout << "Current resolution: " << resolutions[currentResolutionIndex].name
         << " (" << resolutions[currentResolutionIndex].width
         << "x" << resolutions[currentResolutionIndex].height << ")" << endl;

    while (true)
    {
        // 如果预览启用，则捕获并显示图像
        if (previewEnabled)
        {
            for (size_t i = 0; i < cameras.size(); i++)
            {
                if (cameras[i].detected)
                {
                    if (debugMode)
                        cout << "Capturing " << cameras[i].name << " image..." << endl;

                    // 使用当前设置的分辨率进行预览捕获
                    int previewWidth = resolutions[currentResolutionIndex].width;
                    int previewHeight = resolutions[currentResolutionIndex].height;
                    
                    bool success = captureWithLibcamera(cameras[i], cameras[i].tempFile,
                                                        displayImages[i], 300, previewWidth, previewHeight);

                    if (success)
                    {
                        // 如果是第一次预览或者分辨率改变，调整窗口大小
                        if (firstPreview)
                        {
                            resizeWindowToFit(cameras[i].name, displayImages[i]);
                            firstPreview = false;
                        }

                        // 在图像上显示信息
                        Mat displayCopy = displayImages[i].clone();
                        
                        // 使用新的函数添加多行文字信息
                        addImageInfo(displayCopy, 
                                   resolutions[currentResolutionIndex].name,
                                   displayCopy.cols, 
                                   displayCopy.rows,
                                   image_counter);
                        
                        imshow(cameras[i].name, displayCopy);
                    }
                    else
                    {
                        cerr << cameras[i].name << " capture failed" << endl;
                        // 创建黑色图像作为占位符
                        displayImages[i] = Mat::zeros(600, 800, CV_8UC3);
                        string errorText = "Capture Failed";
                        putText(displayImages[i], errorText, Point(10, 30),
                                FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 0, 255), 2);
                        imshow(cameras[i].name, displayImages[i]);
                    }
                }
            }
        }

        // 使用非阻塞方式检测键盘输入
        int key = waitKey(30); // 减少等待时间以提高响应性

        // 同时检测终端输入（解决OpenCV窗口可能不捕获按键的问题）
        int terminalKey = getKeyNonBlocking();
        if (terminalKey != EOF)
        {
            key = terminalKey;
        }

        if (key == 27)
        { // ESC键退出
            cout << "Exiting program..." << endl;
            break;
        }
        else if (key == 'c' || key == 'C')
        { // 保存左右图像对
            cout << "Capturing stereo image pair..." << endl;

            // 生成带零填充的文件名
            ostringstream filename_left, filename_right;
            filename_left << "left_images/left_" << setw(4) << setfill('0') << image_counter << ".jpg";
            filename_right << "right_images/right_" << setw(4) << setfill('0') << image_counter << ".jpg";

            // 获取当前选择的分辨率
            int width = resolutions[currentResolutionIndex].width;
            int height = resolutions[currentResolutionIndex].height;

            cout << "Using resolution: " << width << "x" << height << endl;

            // 捕获并保存图像（使用指定分辨率）
            bool saveSuccess = true;
            Mat saveLeft, saveRight;

            if (cameras[0].detected)
            {
                saveSuccess = captureWithLibcamera(cameras[0], filename_left.str(), saveLeft, 1000, width, height) && saveSuccess;
            }

            if (cameras[1].detected)
            {
                saveSuccess = captureWithLibcamera(cameras[1], filename_right.str(), saveRight, 1000, width, height) && saveSuccess;
            }

            if (saveSuccess)
            {
                cout << "Saved image pair: " << filename_left.str() << " and " << filename_right.str() << endl;
                cout << "Image sizes: Left " << saveLeft.cols << "x" << saveLeft.rows
                     << ", Right " << saveRight.cols << "x" << saveRight.rows << endl;
                image_counter++;
                saved_pairs_count++;
            }
            else
            {
                cerr << "Error: Failed to save image files!" << endl;
            }
        }
        else if (key == 's' || key == 'S')
        { // 保存单张图像(左)
            if (!cameras[0].detected)
            {
                cerr << "Left camera not detected, cannot save image!" << endl;
                continue;
            }

            cout << "Capturing left camera image..." << endl;

            // 生成带零填充的文件名
            ostringstream filename_left;
            filename_left << "left_images/left_" << setw(4) << setfill('0') << image_counter << ".jpg";

            // 获取当前选择的分辨率
            int width = resolutions[currentResolutionIndex].width;
            int height = resolutions[currentResolutionIndex].height;

            Mat saveLeft;
            bool saveSuccess = captureWithLibcamera(cameras[0], filename_left.str(), saveLeft, 1000, width, height);
            if (saveSuccess)
            {
                cout << "Saved single image: " << filename_left.str() << endl;
                cout << "Image size: " << saveLeft.cols << "x" << saveLeft.rows << endl;
                image_counter++;
                saved_left_count++;
            }
            else
            {
                cerr << "Error: Failed to save image file!" << endl;
            }
        }
        else if (key == 'r' || key == 'R')
        { // 保存单张图像(右)
            if (!cameras[1].detected)
            {
                cerr << "Right camera not detected, cannot save image!" << endl;
                continue;
            }

            cout << "Capturing right camera image..." << endl;

            // 生成带零填充的文件名
            ostringstream filename_right;
            filename_right << "right_images/right_" << setw(4) << setfill('0') << image_counter << ".jpg";

            // 获取当前选择的分辨率
            int width = resolutions[currentResolutionIndex].width;
            int height = resolutions[currentResolutionIndex].height;

            Mat saveRight;
            bool saveSuccess = captureWithLibcamera(cameras[1], filename_right.str(), saveRight, 1000, width, height);
            if (saveSuccess)
            {
                cout << "Saved single image: " << filename_right.str() << endl;
                cout << "Image size: " << saveRight.cols << "x" << saveRight.rows << endl;
                image_counter++;
                saved_right_count++;
            }
            else
            {
                cerr << "Error: Failed to save image file!" << endl;
            }
        }
        else if (key == 'd' || key == 'D')
        { // 调试模式
            debugMode = !debugMode;
            cout << "Debug mode " << (debugMode ? "enabled" : "disabled") << endl;
        }
        else if (key == 'p' || key == 'P')
        { // 切换预览
            previewEnabled = !previewEnabled;
            cout << "Preview " << (previewEnabled ? "enabled" : "disabled") << endl;
        }
        else if (key >= '1' && key <= '5')
        {                             // 切换分辨率 (1-5)
            int newIndex = key - '1'; // 将'1'转换为0, '2'转换为1, 等等
            if (newIndex < resolutions.size())
            {
                currentResolutionIndex = newIndex;
                cout << "Switched to resolution: " << resolutions[currentResolutionIndex].name
                     << " (" << resolutions[currentResolutionIndex].width
                     << "x" << resolutions[currentResolutionIndex].height << ")" << endl;
                
                // 分辨率改变后，下次预览需要重新调整窗口大小
                firstPreview = true;
            }
        }

        // 添加短暂延迟，减少CPU使用率
        this_thread::sleep_for(chrono::milliseconds(50));
    }

    // 清理临时文件
    for (const auto &camera : cameras)
    {
        remove(camera.tempFile.c_str());
    }

    destroyAllWindows();

    // 显示实际保存的图像数量
    cout << "Program finished. Total saved image pairs: " << saved_pairs_count << endl;
    if (saved_left_count > 0) {
        cout << "Separately saved left images: " << saved_left_count << endl;
    }
    if (saved_right_count > 0) {
        cout << "Separately saved right images: " << saved_right_count << endl;
    }
    cout << "Next image index: " << image_counter << endl;
    
    return 0;
}